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A Small AUV Navigation System (SANS) is being developed at the Naval 
Postgraduate School. The SANS is an integrated INS/GPS navigation system composed 
of low-cost, small-size components. It is designed to demonstrate the feasibility of using 
a low-cost Inertial Measurement Unit (EVfU) to navigate between intermittent GPS fixes. 

This thesis presents recent improvements to the SANS hardware and software. 
The 486-based ESP computer used in the previous version of SANS is now replaced by 
an AMD 586DX133 based PC/104 computer to provide more computing power, 
reliability and compatibility with PC/104 industrial standards. The previous SANS 
navigation filter consisting of a complementary constant gain filter is now aided by an 
asynchronous Kalman filter. This navigation filter has six states for orientation 
estimation (constant gain) and eight states for position estimation (Kalman filtered). 
Low-frequency DGPS noise is explicitly modeled based on an experimentally obtained 
autocorrelation function. Ocean currents are also modeled as a low-frequency random 
process. The asynchronous nature of DGPS measurements resulting from AUV 
submergence or wave splash on the DGPS antennas is also taken into account by 
adopting an asynchronous Kalman filter as the basis for the SANS software. Matlab 
simulation studies of the asynchronous filter have been conducted and results 
documented in this thesis. 
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I. INTRODUCTION 



A. GENERAL 

In vehicle navigation systems, integrated inertial measurement devices are 
commonly used to compensate for the loss of Global Positioning Systems (GPS) signals 
due to environmental blockages. These devices have been in use for some time in 
aircraft, ships, and large land vehicles. In the past, inertial information has been derived 
from expensive gyros that required large spaces relative to the size of the vehicle. As the 
size of the vehicles gets smaller, implementing these kinds of integrated devices can be 
quite expensive. Thus, research is being conducted on ways to compensate for this GPS 
signal loss with low cost off-the-shelf components. Current chip technology involving 
micromachined silicon has allowed progress in this particular area of navigation systems. 
The goal of this thesis is to show that a low cost integrated (Inertial Navigiation System) 
INS/GPS navigation system can be developed using commercially available components. 

B. BACKGROUND 

At the Naval Postgraduate School (NPS), a small autonomous underwater vehicle 
(AUV) known as Phoenix requires accurate navigation to report the position of shallow 
water mines or monitor the coastal environment [Ref 3]. In the course of its missions, 
the vehicle periodically surfaces to obtain a Differential Global Positioning System 
(DGPS) fix and then submerges. While underwater, the Phoenix uses gyros, a depth 
sensor, a speed sensor, and compass heading to estimate its position. Once a shallow 
water mine has been detected with sonars, the location of the mine can be estimated 
based on the position of the AUV. Previous hardware and software were tested with 
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promising results. The current Phoenix navigation system has two faults. One, it does 
not account for water current, and two, its gyros are costly, bulky, power hungry, and 
prone to failure. 

Over the last seven years, research has gone into solving these problems and the 
Small AUV Navigation System (SANS) is the outcome of this work. SANS, now in its 
third generation of development, has undergone a number changes [Ref 3]. 

SANS technology is also becoming a more attractive solution to human motion 
tracking. Since the size of its components is getting smaller with each generation, 
improved portability is making it possible to track a human carrying a SANS device. For 
example, a small law enforcement team carrying SANS might enter a building with their 
location being sent back to a remote location outside the building so tacticians could 
better evaluate situations. 

As suggested in the previous examples, the application of a device such as SANS 
is limitless. In this thesis, work has been narrowed to AUV navigation, at the same time 
knowing that applications beyond Phoenix are possible. 

C. DESIGN TASKS 

• Replace SANS computer; 

• Replace Inertial Measurement Unit; 

• Replace DGPS unit; 

• Replace magnetometer unit; 

• Replace speed sensor; 

• Develop an asynchronous Kalman filter for SANS; 

• Conduct simulation of the asynchronous Kalman filter for SANS; 
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• Test and evaluate any new components. 

D. SCOPE, LIMITATIONS, AND ASSUMPTIONS 

This thesis reports part of the findings of the seventh year of research in an 
ongoing research project. A new hardware configuration for SANS and a simulation of 
the SANS asynchronous Kalman filter are the main outcomes of this work. The scope of 
this thesis is to develop a new navigation system (SANS) for eventual installation as a 
replacement for the expensive and older technology gyros now used onboard the NFS 
Phoenix AUV. 

E. ORGANIZATION OF THESIS 

The purpose of this thesis is to present a prototype hardware platform and the 
filter theory required to meet the mission requirements of SANS. In this thesis, the term 
AUV is understood to include any small underwater vehicle (including humans) which 
can easily carry such a compact device (SANS). The major thrust of this thesis is to 
describe the latest developments of the SANS hardware and offer an optimal filter as a 
replacement to the original complementary filter. 

Chapter II presents an overview of the SANS theory of operation and an 
explanation of the navigation filter. Chapter III presents the previous SANS hardware 
configuration and its current state. Chapter IV reviews Kalman filter basics, describes 
the SANS asynchronous Kalman filter equations and presents simulation results. Chapter 
V presents the thesis conclusions and provides recommendations for future research. 
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II. SANS THEORY OF OPERATION 



A. INTRODUCTION 

The objective of the SANS is to produce real time navigation information by 
integrating an Inertial Navigational System with Differential Global Positioning System 
(DGPS). Previous work on the SANS filter discussed in [Ref 3,5 and 6] consists of a 14- 
state complementary filter designed to estimate north-east position. This chapter explains 
the theory of inertial navigation, real world problems involved with this type of 
navigation, and how SANS uses this theory to deliver navigation information. 

B. INERTIAL NAVIGATION THEORY 

To navigate between GPS fixes, SANS uses inertial sensors that measure linear 
accelerations in three orthogonal axes and angular rates about three orthogonal axes. In a 
perfect world where errors do not occur in sensors, angular rate could be integrated to 
give angular position or orientation. Integrating the acceleration twice would give 
position. Thus, position could be obtained in three dimensions given acceleration in three 
orthogonal axes. Theoretically, with no noise in sensor measurement, orientation and 
position could be found using only an Inertial Measurement Unit (IMU) given an initial 
attitude and position. 

C. REAL WORLD PROBLEMS WITH INERTIAL NAVIGIATION 

In practice, low cost angular rate sensors and linear accelerometers are prone to 

drift. Estimating position and orientation with an IMU requires knowledge of the drift 

characteristics of the sensors and how to correct them. Using sensors that are more 

accurate drives costs into the tens of thousands of dollars. Thus, a method is needed to 
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account for sensor drift. Angular rate sensor data are used as high frequency data and 
integrated once to obtain orientation information in a short period of time. Accelerometer 
data are used as low frequency data to correct drift in orientaion estimation. [Ref 2] 
Integrating linear acceleration twice gives linear position. Integration causes 
errors in position estimation to grow quadratically. DGPS and waterspeed sensors are 
used as low frequency devices to correct drift in position estimation. Once acceleration is 
integrated to give velocity, errors within this integration is corrected with waterspeed. 
When velocity is integrated to give position, DGPS is used correct position errors. 
Differences between DGPS position and IMU position are attributed to ocean current. 
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Figure 2.1: Previous SANS Filter 
D. SANS NAVIGATION 

SANS uses the principles of inertial navigation to estimate attitude. GPS and 
waterspeed data aid in position estimation. The previous SANS filter (Figure 2. 1) is a 
14-state complementary filter. Attitude estimation transforms all the body rates and 
angles of the AUV to earth-based coordinates, namely, north-east-down. The T 
transformation matrix in Figure 2. 1 converts body angular rates to Euler rates and the R 
tranformation matrix transforms body-based linear accelerations and velocities to earth- 
based angles. Six states account for attitude, three for angular rate and three for 
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acceleration. The other eight states are north-east position, velocity, ocean current, and 
GPS bias. Position in the down axis is obtained using a pressure sensor. This thesis only 
focuses on the north-east coordinate estimation. The eight states were determined using 
constant gains for error correction. 

SANS obtains accurate navigation information by integrating IMU data and 
DGPS data. While IMU data is sampled periodically, DGPS is available aperiodically 
due to asynchronous reacquisition time of satellite signals and asynchronous 
submergence-surfacing duration of the AUV. To optimally integrate IMU and DGPS 
data, an asynchronous Kalman filter is an ideal method. 



DGPS 



Kalman 

Filter 



Water Speed 




North Velocity 
East Velocity 
North Current 
East Current 
North GPS Bias 
East GPS Bias 
North Position 
East Position 



IMU f 






Euler Angles 



Compass 



Figure 2.2: Current SANS Filter 



Figure 2.2 shows the how eight states would be used in an asynchronous Kalman filter. 
The EMU and compass give the necessary Euler angles to break waterspeed into north- 
east vector using the AUV body to earth based coordinate R rotation matrix. Chapter IV 
goes into more detail as to how the filter was developed. 
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III. SANS SYSTEM CONFIGURATION 



A. INTRODUCTION 

The objective of SANS is to develop a low cost way to navigate between DGPS 
fixes using an EMU. The system should be small enough to fit into the Phoenix AUV. 
References [2, 3, 5, 6, and 7] described the previous SANS. This thesis presents the current 
SANS. Most parts of the previous unit have been replaced with cheaper, smaller, and 
more powerful components. This chapter briefly describes the previous SANS and then 
details the current SANS configuration and the operation. 

B. PREVIOUS SANS HARDWARE 

The previous SANS (Figure 3.1) consisted of a 486SLC DX2 CPU module, an 
A/D converter, Systron Dormer Inertial Measurement Unit (EMU), a waterspeed sensor , a 
Motorola ONCORE 8-channel receiver with an imbedded DGPS capability, and a 
Precision Navigation TCM2 Electronic Compass . These components were installed in a 
box measuring 6x17x3 inches (306 cubic inches). The previous version of SANS 
sampled at 40 Hz and required 12 VDC power. The electronic compass and power 
supply were located external to the box to reduce magnetic interference. 
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Unregulated 12 VDC 




Figure 3.1: Previous SANS Configuration 



C. CURRENT SANS HARDWARE 

The current SANS configuration (Figure 3.2) is smaller, faster, and more flexible. 
The DGPS and electronic compass were kept from the old system. All other components 
were upgraded. This version of SANS is capable of sampling at an estimated rate of 100 
Hz and requires only 5 VDC to operate. The current SANS configuration is 
approximately 150 cubic inches excluding the electronic compass and the power supply. 
The new components are a Real Time Devices 133 MHz AMD 586 PC/104, a Sealevel 
four port RS-232 module, a Crossbow six axis inertial measurement unit, and a SonTek 
Hydra water speed sensor. 
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Regulated 5 VDC 

i ^ 




Figure 3.2: Current SANS Hardware Configuration 



1. Real Time Devices DSV586DX133 PC/104 

The Real Time Devices PC/104 module will process all data received from the 
sensors. Utilizing to the PC/104 standard adds flexibility to the system. The PC/104 
CPU is small, relatively cheap at $600 per CPU and easy to expand with stacking 
modules. Over the past decade, this PC architecture has become an accepted platform for 
dedicated and embedded applications. PC/104 offers the full architecture, hardware and 
software compatibilities of the standard PC bus, but in compact (3.6 inches x 3.8 inches) 
stackable modules. [Refs. 8 and 14] 

The Real Time Devices PC/104 has powerful features for its size. It operates on 5 
VDC, as do all the other SANS components. Also, this PC/104 has a 12 MB disk on a 
chip (expandable to 72 MB) that will store the SANS code and any data it accumulates. 

A Integral Viper 170 MB hard drive is also available for the PC/104 if more storage is 
required, i.e. navigation data for post processing. [Ref 8] 



RS-232 



RS-232 



GPS/DGPS 

Receiver 



IMU 
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2. Four RS-232 Port Serial I/O Module 



Two serial ports usually come standard on PC's. However four serial ports were 
needed in SANS. The Sealevel C4-104 serial I/O module provides SANS with four serial 
ports each with its own memory addresses and interrupt request assignments. All the 
sensors, the IMU, DGPS, water speed sensor, and electronic compass output to a RS-232 
format. By using this module, an A/D board is not necessary to convert analog sensor 
data to a digital representation. Most importantly, removing the A/D board from SANS 
reduces its size by approximately 60 cubic inches. [Ref 9] 

3. Crossbow DMU-VG Six Axis Inertial Measurement Unit 
The Crossbow DMU-VG six axis IMU consists of three rate gyros, three 
accelerometers, and a 12 bit A/D board all in a 3 x 3.375 x 3.250 inch box. It operates in 
one of two output modes, a scaled sensor mode and a voltage mode. In sensor mode, roll 
and pitch can be obtained directly from an on-board processor. This capability reduces 
the amount of computing for the PC/104. The original SANS code calculated the roll and 
pitch from the angular rates and acceleration readings of the Systron Donner IMU [Ref 
2]. The Crossbow unit produces a data packet (Table 3.1) which can be used to replace 
this portion in the SANS code. [Ref 10] 
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Note; Most Significant Bit (MSB) Least Significant Bit (LSB) 



0 


Header (255) 


1 


Roll (MSB) 


2 


Roll (LSB) 


3 


Pitch (MSB) 


4 


Pitch (LSB) 


5 


Roll Rate X (MSB) 


6 


Roll Rate X (LSB) 


7 


Pitch Rate (MSB) 


8 


Pitch Rate Y (LSB) 


9 


Yaw Rate Z (MSB) 


10 


Yaw Rate (LSB) 


11 


Acceleration X (MSB) 


12 


Acceleration X (LSB) 


13 


Acceleration Y (MSB) 


14 


Acceleration Y (LSB) 


15 


Acceleration Z (MSB) 


16 


Acceleration Z (LSB) 


17 


Temp Sensor Voltage (MSB) 


18 


Temp Sensor Voltage (LSB) 


19 


Time (MSB) 


20 


Time (LSB) 


21 


Checksum 



Table 3.1: Crossbow IMU Data Packet Format [Ref. 9] 



4. SonTek Hydra Water Speed Sensor 

The SonTek Hydra water speed sensor is a single point, high resolution, 3D 
Doppler current meter. This device uses one transmitter and 3 acoustic receivers which 
are aligned to intersect with the transmit beam pattern at a common sampling volume. 
The velocity measured by each receiver is referred to as the bistatic velocity, and is the 
projection of the 3D velocity vector on the bistatic axis of the acoustic receiver. An RS- 
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232 output makes intergration with the PC/104 simple. What makes this water sensor 
unique is its ability to report velocity data in the Earth (North-East-Down) coordinate 
system with the aid of an internal compass and tilt sensor. The velocity in Earth 
coordinates is fed directly into the asynchronous Kalman filter described in the next 
chapter. [Ref 11] 

5. McKinney Technology DGPS Receiver 

This GPS/DGPS receiver package is specially designed to operate in the 
Monterey Bay area. The contained Motorola Oncore receiver is capable of tracking up to 
eight satellites simultaneously. It can provide position accuracy of better than 24 meters 
Spherical Error Probable (SEP) without Selective Availability (SA) and 10 meters (SEP) 
with SA. Typical-Time-To-First Fix (TTFF) is 18 seconds with a reacquisition time of 
2.5 seconds. [Ref 12] 

6. Precision Navigation TCM2 Electronic Compass Sensor Module 

The TCM2 consists of a three-axis magnetometer and a two-axis tilt sensor with a 
small A/D board to output roll, pitch, heading, and a three dimensional magnetic field 
measurement. It is accurate to within one half of a degree in level operation. Reference 
[5] describes calibration of the compass and its error characteristics. Its size is 2.5 x 2 1.1 
inches. It requires 5 VDC and 15-22 mA. [Ref 13] 

D. FUTURE SANS HARDWARE 

As of this writing research is proceeding regarding other possible components of 
SANS. A method to transmit data from SANS to a remote location is desired. This has 
led to the purchase of the Proxim RangeLAN2 7402 PC card. The RangeLAN2 is 
capable of transmitting data at 1.6 Mbps through a PCMCIA type II card format at 
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distances up to 1000 feet. The SANS PC/104 already comes with a PCMCIA module 
that fit 2 cards. With this feature, data received from the sensors or processed navigation 
information can be observed remotely. [Ref 15] 

Research into a smaller IMU showed that a device with a three axis 
accelerometer, three axis angular rate sensor, and three axis magnetometer all in a 
package the size of wrist watch is approximately two years away. Demand for such a 
device is slowly increasing, but not fast enough for the commercial industry to mass- 
produce them at this time. Finally, the current SANS DGPS module is three years old. 
Enough advances have been made in the GPS technology that would warrant changing 
out the DGPS package for a system that is smaller and more accurate. Thus far, research 
has offered two possible products. One is Rockwell Semiconductor's NAVCARD LP, 
which comes in a PCMCIA card format [Ref 16]. The other is Trimble's Pathfinder with 
ASPEN software also in a PCMCIA card format [Ref 17], No information was found on 
the DGPS radio receivers integrated with GPS on PCMCIA cards. User defined settings 
of the receiver frequency are set for the area of GPS operation. 

As with any hardware device, its effectiveness is usually determined by the 
quality of its software. The SANS code [Ref 2] is currently written in C++ and utilizes 
the constant gain complementary filter method found in Chapter El. Using the 
configuration described in this chapter, the next task is to implement an asynchronous 
Kalman filter to integrate the sensor data. The result of this filter work is described in 
Chapter IV. 
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IV. SANS ASYNCHRONOUS KALMAN FILTER 



A. INTRODUCTION 

This chapter discusses the development of a discrete time asynchronous Kalman 
filter that estimates the eight states of the SANS model after attitude estimation. A brief 
discussion will be provided on Kalman filter basics leading into a derivation of the SANS 
Kalman filter equations. Finally, results of the SANS asynchronous Kalman filter Matlab 
simulation will be presented. 

B. KALMAN FILTER BASICS 

The discrete Kalman filter is a recursive predictive update technique used to 
estimate the states of a process model. This state-space filter method has two main 
features (1) vector modeling of the random processes under consideration and (2) 
recursive processing of the noisy measurement (input) data [Ref 1]. Given some initial 
estimates, it allows the states of a model to be predicted and adjusted with each new 
measurement, providing an estimate of error at each update. It incorporates the effects of 
measurement noise and modeling noise in its computational structure. 

A model of a random process can be described as; 

(4.1) 

In cases where the relationship between model and its measurements is linear, the 
observation (measurement) of the random process model (Eq. 4.1) becomes: 

Z* = H;tX, + (4.2) 
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Throughout the remainder of this chapter, various terms are used to better explain the 

Kalman filter. The following notations are consistent with Reference [1]; 

(n X 1) Vector containing actual states of a process model at time . 

X* (n X 1) Vector containing the current estimated states at . 
xj X 1) Vector containing the estimated states at time t before updating with 

measurement. 

x’„, (n X 1) Vector containing the estimated states at the next time sample. 

(n X n) Matrix containing the error covariance for X;^ . 

P‘ (n X n) Matrix containing the error covariance for i* . 

P'^, (n X n) Matrix containing the error covariance for xj„, . 

(m X 1) Vector containing the measurement at time . 

(m X n) Matrix giving the ideal (noiseless) connection between the measurement 
and the state vector at time . 

(m X n) Matrix denoting the measurement error covariance, which must be known 
or estimated a priori. 

(j)^ (n X n) Matrix containing the model relating x^and x‘^, in the absence of a forcing 
function ( if x^^ is a sample of a continuous process, <j>^ is the usual state transition 
matrix). 

w^, (n X 1) Vector whose elements are white sequences with known covariance 
structure to be used with . 

(m X 1) Vector whose elements are the measurement errors associated with z^ - 
assumed to be a white sequence with known covariance structure and having zero 
cross-correlation with the sequence. 

(n X n) Matrix containing the covariance of w*. . 

K;t (n X n) Kalman Gain matrix that relates the amount of influence that the error 
between x' and z* has in deriving x^ . 

q white noise variable with zero mean and variance of element being modeled. 



The discrete Kalman filter basically contains five recursive equations. Beginning 
with a prior estimate, the noisy measurement z* is used wth a blending factor K*(yet to 
be determined) to improve the the estimate as follows; 

X* = x; + K*(z,- H, i;^) (4.3) 
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(known as the Kalman gain) is now needed to find the optimal estimate . It 
takes the error covariance (mean-square error) between the current state and the 
estimated state and applies it with and resulting in 

K, =p;h[(h.p;h[+r.)-' (4.4) 

Once the Kalman gain minimizes the mean-square estimation error, a new 
covariance matrix can be computed for (Eq. 4.3) with 

P,=(I-K,H,)P; (4.5) 

The updated estimate x* is projected ahead using the state transition matrix . 
Unlike Eq. (4.1) the noise vector does not affect the projected states because it has 
zero mean and is white (zero correlation with any previous w^. ). Thus, 

K.i = (4-6) 

Finally, the projected error covariance for x^^, uses the updated error covariance 
from Eq. (4.5), the covariance of in Eq. (4. 1), denoted as , and (j>i^ the state 
transition matrix to form the following: 

p;„=«>.p.A’'+Q. (4.7) 

Equations (4.3 - 4.7) can be placed into an algorithm that can loop infinitely. This 
Kalman filter loop is shown in Figure (4.1). 



19 



Enter prior estimate x‘ and its error covariance Po 

I 




Figure 4.1: Kalman filter loop from Ref. [1] 




Figure 4.2: SANS Process Model (after attitude estimation) 



20 



C. DERIVATION OF SANS KALMAN FILTER EQUATIONS 

The SANS asynchronous Kalman filter consists of eight states, north position, 
east position, north velocity, east velocity, north current, east current, north GPS bias, and 
east GPS bias. Figure 4.2 shows the process model developed by the SANS team [Ref 
4]. This model contains the eight states known as the vector x*. Equations 4.8 through 
4.15 show the derivation of the state equations for the SANS process model. 



1 1 

= — ^ 1+— ^1 
^1 ^1 


(4.8) 


1 1 
= ^1 + — ^2 


(4.9) 


1 1 

= ^+—^3 

Tj T2 


(4.10) 


+ 

1 

II 


(4.11) 


1 1 

= 

h h 


(4.12) 


1 1 
= ^6 


(4.13) 


= X, + X3 


(4.14) 


Xj = Xj + x^ 


(4.15) 



From these equations, (f> (state transition matrix) was developed. 
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The next item needed for the Kalman filter was the Q* matrix. Equation 4. 16 is 
the equation for Q*, the mean of model noise squared. 

Wk is the plant noise with covariance Q^, 

Q, =£[w,wj] (4.16) 
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and Vfc is measurement noise with covariance R, 



R = 



.5 0 0 0 

0 .5 0 0 

0 0 0 0 

0 0 0 0 



with DGPS signal 



R = 



.5 0 
0 .5 



without DGPS signal 



0 

0 

0 

0 

0 

0 

0 

0 



The H matrix is the noiseless connection between measurement and the state vector a 
time t. For the SANS asynchronous Kalman filter, two H matrices describe this 
connection, one for samples with DGPS the other for samples without DGPS. 



1 0 0 0 0 0 
0 1 0 0 0 0 
0 0 0 0 1 0 
0 0 0 0 0 1 



0 

0 

1 

0 



0 

0 

0 

1 



with DGPS signal 
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without DGPS signal 



H = 



1 

0 



0 0 0 0 0 0 0 
10 0 0 0 0 0 



Using the main components of the Kalman filter in the previous section, the filter was 
tested in Matlab. 

D. SANS KALMAN FILTER SIMULATION 

The SANS asynchronous Kalman filter was coded in MATLAB® [Ref 18] by 
applying the matrices developed in section C to the prediction and correction equations as 
outlined in Equations 4.1 through 4.7. Simulation results of the SANS Kalman filter are 
presented in the following section. The source code for the SANS Kalman filter is 
presented in Appendix A. 

The model is assumed stationary throughout the simulation. Noise is generated 
from the process model and measurement errors. The following figures show the results 
of simulation over a six-minute period sampling at a rate of 100 Hz. 

Figure 4.3 shows the north position versus time result in the six-minute 
simulation. The A symbols represent the colored noise sampling of DGPS position. The 
continuous line represents the actual model position, and the dotted line represents the 
estimated Kalman filter position of the model. 
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North Position vs Time 




— Actual Position 
Estimated Position 
A DGPS Measurement 



50 100 150 200 

time (seconds) 



250 



300 



350 



Figure 4.3: Plot of North Position vs. Time 

Notice the initial jumps in the estimated position (dotted line) and how the 
estimate approaches the actual model state within the first 200 seconds although GPS 
information is drifting off. This behavior shows that the Kalman gain for GPS is 
decreasing and more reliance is being given to velocity measurement. 
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East Position vs Time 




Figure 4.4: Plot of East Position vs. Time 

Similar to Figure 4.3, Figure 4.4 displays estimated east position against time. This plot 
demonstrates GPS fluctuating about zero but the Kalman filter is able to maintain a good 
estimate of position given a zero velocity measurement. In addition to plotting position 
versus time, the error covariance matrix P* is presented in Appendix B for examination of 
error covariance and correlation among each of the states in the Kalman filter. 
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Estimated North Position vs Estimated East Position 




Figure 4.5: Plot of Estimated North Position vs Estimated East Position 

Figure 4.5 represents data from Figures 4.3 and 4.4 and plots them together to 
give a more geographic plot of the simulation model. The long jumps in position were 
due to initial estimates within the first 30 seconds. As time went on, the position 
differential became minimal. It can be seen that an asynchronous Kalman filter is a good 
way to optimally estimate the states of SANS. This case only presents the position 
information, but all the other states can be easily represented. Much more can be done 
with this simulation, such as including all the states of the SANS filter and getting better 
models for measurement data (GPS and waterspeed). 
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V. CONCLUSIONS 



A. SUMMARY 

The purpose of this thesis was to develop a low cost method to navigate a small 
AUV by integrating an inertial navigation system and differential GPS Major 
modifications have been made to SANS and the navigation filter now optimally estimates 
position. SANS was reduced by 52% in size and capable of running at an estimated 100 
Hz sampling rate. The components in SANS were selected based on cost, size, and ease 
of operation. An asynchronous Kalman filter was developed and tested in simulation 
with promising results. What makes this navigation filter unique from any other AUV 
navigation system is that it accounts for current. The Kalman filter explained in this 
thesis estimates the AUV position, velocity, ocean current, and DGPS bias. 

B. FUTURE RESEARCH 

The future of SANS has many avenues predicated upon technology advances, 
software development, and the amount of research put into testing and evaluation. As 
micromachined chip technology evolves in the inertial navigation field, improvements in 
IMUs will most certainly come about. Reducing the size of SANS should continually be 
a goal while maintaining performance and low cost. Using the PCMCIA Card format for 
GPS receivers could potentially reduce the size of SANS by another 30 cubic inches. 

The limiting factor with PCMCIA GPS receivers is the added size of a differential signal 
receiver required for DGPS. No DGPS receivers have been implemented onto a single 
board. 
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Addition of the Proxim RangeLAN2 PC card [Ref 15] gives SANS another 
dimension of usability. This card could give SANS more portability and potentially 
changes the way the sensors are integrated and utilized for applications other than AUVs. 
For example, data coming from the IMU, compass, DGPS, and water speed sensor could 
be transmitted to a remote computer for post processing rather than taking up CPU time 
running the navigation software. 

Now that the asynchronous Kalman filter has been developed, more tests are 
needed to further test its reliability. A better DGPS model could be used for simulations 
that are more realistic. In addition, simulated velocities and currents could provide 
further assurance that the states are being estimated correctly. Another area needing 
work is the navigation software. The original SANS code is in C++ and operated on a 
Maxus ESP 50 MHz PC. A new SANS code needs to work on the PC/104 platform using 
the new EMU and waterspeed data format. The code could be in C++ or Java. Using 
Java gives SANS more flexibility within the network of computers imbedded within the 
Phoenix AUV. 

Currently, research is being done on using quaternions vice Euler angles for 
attitude estimation. Use of quaternions would be necessary for SANS to track the 
motions of human limbs through the vertical. Accomplishing this task would also mean 
reducing the size of SANS to that of a wristwatch, which would be strapped onto various 
body segments. For more information see Reference [19]. Tilt table tests must be 
performed to examine the EMU data. Compass calibration has already been examined 
[Ref 6]. When SANS reaches the stage when hardware and software are fully integrated 
ground tests and sea trials will be needed to prove its operation. 
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APPENDIX A: SANS ASYNCHRONOUS KALMAN FILTER MATLAB CODES 



clear all 



% SANS Kalman Filter 
% By Glenn Hernandez 
% 8 May 98 

% Number of minutes for simulated run 
minutes = 6; 



% Define the resolution of the simulation here 
delta t = .01; % For 100 Hz resolution 



% Number of samples 

samples = l/delta__t * 60 * minutes; % Gives 60000 samples at delta_t = 
.01 seconds 



% Time Constants 
tau_l =60; % seconds 

tau_2 =60; % seconds 

tau 3 = 3600; % seconds 



for velocity 
for GPS 

for ocean current 



% Process Noise Vector 

wl = randn( 1, samples ) ; 

w2 = randn( 1, samples) ; 

w3 = randn ( 1/ samples ) ; 

w4 = randn ( 1, samples ) ; 

w5 = 600 * randn( 1 , samples ) ; % 

w6 = 600 * randn(l, samples) ; % 

w7 = zeros ( 1, samples) ; % 

w8 = zeros ( 1, samples) ; % 

w = [wl;w2;w3;w4;w5;w6;w7;w8] ; 



Gives a GPS standard 
Gives a GPS standard 
No white noise input 
No white noise input 



deviation 
deviation 
for x7 
for x8 



of 

of 



3 m 
3 m 



% Measurement Noise Vector 
vl = randn ( 1, samples ) ; 
v2 = randn ( 1, samples ) ; 
v3 = randn{ 1, samples) ; 
v4 = randn ( 1, samples) ; 

v_0 = (vl;v2]; % Noise vector without GPS input 

v_l = [ vl ; v2; v3; v4 ] ; % Noise vector with GPS input 



% Generate GPS Sampling 
gps_flag = zeros ( samples, 1 ) ; 

for g = 1 : l/delta_t: samples % Need at least .01 seconds between GPS 
fixes 



j = rand; % Normalized random generator between 0 and 1 
if j < .5 

gps_flag(g) =0; % No GPS Signal Available 
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else 

gps_flag(g) = 1; % GPS Signal Available 
end 
end 



*gps flags generated* 



% System Matrix 










A = [l/tau_l 


0 


0 


0 


0 


0 


0 


1/tau 1 


0 


0 


0 


0 


0 


0 


1/tau 2 


0 


0 


0 


0 


0 


0 


1/tau 2 


0 


0 


0 


0 


0 


0 


1/tau 3 


0 


0 


0 


0 


0 


0 


1/tau 3 


1/tau 


_1 0 


l/tau_2 


0 


0 


0 


0 


l/tau_l 


0 


1/tau 2 


0 


0 



0 

0 

0 

0 

0 

0 

0 

0 



% Input Noise Matrix 
B = [ wl; w2 ; w3;w4 ;w5 ; w6;w7 ;w8 ] ; 



% Output Matrix 
C = eye{8) ; 



D = zeros (8, samples) ; 

% State Transition Matrix 

phi = [exp(-delta_t/tau_l) 0000000 ; 

0 exp(-delta_t/tau_l) 000000; 

0 0 exp ( -delta_t/tau_2 ) 00000 ; 

000 exp( -delta^t /tau_2 ) 0000; 
0000 exp(-delta_t/tau_3) 000; 
00000 exp( -delta_t/tau_3 ) 00; 
tau_l* ( 1-exp (-delta_t/tau_l) ) 0 
tau_2* ( 1-exp (-del ta_t/tau_2 ) ) 00000; 
0 tau_l* ( 1-exp ( -delta__t/tau_l ) ) 

0 tau_2 *( 1-exp (-delta_t/tau_2 ) ) 0 0 0 0]; 



0 

0 

0 

0 

0 

0 

0 

0 ] 



Q = [ ( (1/ (2*tau_l) ) * (l-exp( (-2*delta_t) /tau_l) ) ) 0000000 

0 ( (l/(2*tau_l) )*(l-exp{ (-2*delta_t)/tau_l) ) ) 000000 
0 0 ( (l/(2*tau_2) )*(l-exp{ (-2*delta_t) /tau_2) ) ) 00000 
000 ( (1/ (2*tau_2) ) * ( l-exp( (-2*delta_t) /tau_2 ) ) ) 0000 
0000 ( (l/(2*tau_3) )*(l-exp( (-2*delta_t)/tau_3) ) ) 000 
00000 ( ( 1/ ( 2*tau_3 ) ) * ( l-exp( (-2*delta_t ) /tau_3 ) ) ) 0 0 
00000 0 00 

00000 0 00 ] 



% Generate Process Noise Vectors 
process_noise = sqrt(Q)*w; 

% Error Covariance Matrix 

R_0 = diag{[.5 .5)); % Without GPS signal 

R_1 = diag([.5 -500]); % With GPS signal 
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% Generate Measurement Noise Vectors 

sensor_noise_0 = sqrt(R_0) * v_0; % Without GPS signal 
sensor_noise_l = sqrt(R_l) * v_l; % With GPS signal 



% Initial x_hat_minus 
x_hat_minus (8, samples ) = zeros; 



% Initial x_hat_plus 
x_hat_plus ( 8, samples ) = zeros; 

% Initial State Vector 
x( 8, samples) = zeros; 



% Initial Error Covariance 



P^minus = [ . 5 
0 
0 
0 
0 
0 
0 
0 



0 0 0 0 
.5 0 0 0 

0 10 0 
0 0 10 
0 0 0 3 

0 0 0 0 
0 0 0 0 
0 0 0 0 



Matrix 
0 0 0 
0 0 0 
0 0 0 
0 0 0 
0 0 0 
3 0 0 

0 5 0 

0 0 5) 



time_index_a = 1; % Initial Index 

time index b = 1; % Initial Index 



for Measurement Vector without GPS 
for Measurement Vector with GPS 



’Beginning Kalman Loops' 



% Begin Simulation 
for k = 2: samples 



% Generate State Vectors and Measurement Vectors 
x(:,k) = phi * x(:,k-l) + process_noise ( : , k~l ) ; 



% Kalman loop with out GPS signal 
if gps_flag(k) == 0 



H= (10000000 ; 
01000000 ]; 



R = diag([.5 .5]); 



sensor_noise_0 ( : , k) = sqrt(R) * v_0(:,k); 

z^vell ( t ime_index_a) = H(l,:) * x(;,k) + sensor^noise^O ( 1 , k) ; 
z_vel2 ( time_index__a) = H(2,:) * x(:,k) + sensor_noise_0 ( 2 , k) ; 

z_vel_time ( time_index_a) = k * delta_t; 

z_vel = [z_vell ; z_vel2]; 

% Compute Kalman Gain 

K = P_minus * H' * inv(H * P_minus * H' + R) ; 
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% update Estimate 

x_hat_plus ( : , k) = x_hat__minus ( : , k-1 ) + K ^ 

( z_vel ( : , time_index_a ) - H * 
x_hat_minus ( : , k-1 ) ) ; 



% Compute Error Covariance for Updated Estimate 
P^plus = ( eye{8) - K * H ) * P^minus; 

P_plus = ( P_plus + P_plus ' ) / 2; 



time_index_a = time_index_a + 1; % Increase the measurement 

vector index by 1 

else 



H = [10000000 ; 
01000000 ; 
00001010 ; 
00000101 ]; 

R = diag([.5 .5 0 0] ) ; 



z_gpsl ( t ime_index_b ) 
z_gps2 ( t ime index_b ) 
z_gps3 ( time_index_b) 
z_gps4 ( t ime_index_b ) 



= H(l,:) * x(:,k) 
= H(2,:) * x(:,k) 
= H(3,;) * x(:,k) 
= H(4,:) ^ x(:,k) 



+ sensor_noise_l ( 1 , k) 
+ sensor_noise_l ( 2 , k) 
+ sensor_noise_l ( 3 , k) 
+ sensor_noise_l ( 4, k) 



z_gps = [z_gpsl ; z_gps2 ; z_gps3 ; z_gps4 ] ; 
z_gps_time ( time_index_b) = k ^ delta_t; 

% Compute Kalman Gain 

K = P_minus * H* * inv(H * P_minus H * + R) ; 



% Update Estimate 

x_hat_plus ( : , k) = x_hat_minus { : , k-1 ) + K 

(z_gps{ : , time_index_b) - H * 
x_hat_minus ( : , k-1 ) ) ; 



% Compute Error Covariance for Updated Estimate 
P_plus = ( eye (8) - K * H ) * P_minus; 

P__plus = ( P_plus + P_plus* ) / 2; 

time_index_b = time_index_b +1; % Increase the measurement 

vector index by 1 



end 

pmin(:,:,k) = P_plus; % Save Matrix for future analysis 



34 



% Project Ahead 

x_hat_minus ( : , k) = phi * x_hat_plus ( ; , k) ; 
P_minus = phi * P_plus * phi* + Q; 

P_minus = ( P_minus + P_minus* ) / 2; 

time(k) = k * delta_t; % Memorize time index 

end 



figure ( 1 ) 
subplot (3,1,1) 

plot (time, x(7,:), *b-*,time, x_hat_minus ( 7 , : ) , * r- * * , z_gps_time , 

z_gps3, 'g*') 

xlabel('time (seconds)*) 

ylabel (* posit ion (meters)*) 

title( *North Position vs Time*) 

axis([0 max (time) -1. 5*max ( abs ( z_gps3 ) ) 1 . 5*max ( abs ( z_gps3 ) ) ] ) 
subplot (3, 1,2) 

plot (time, x(8,:), *b-*,time, x_hat_minus ( 8, : ) , * r- . * , z_gps_time, 

z_gps4, ’g-’) 

xlabel(*time (seconds)*) 

ylabel ( *position (meters) * ) 

title( *East Position vs Time*) 

axis([0 max (time) -1. 5*max ( abs( z_gps4) ) 1 . 5*max ( abs ( z_gps4 ) ) ] ) 
subplot (3,1,3) 

plot ( x_hat_minus ( 7 , : ) , x_hat_minus ( 8 , : ) , * b- * ) 

xlabel (* North Position*) 

ylabel ( * East Position * ) 

title (* North-East Position Plot*) 

axis( [-1. 5*max(abs(x_hat_minus(7, : ) ) ) 1 . 5*max ( abs (x_hat_minus ( 7 , :)))..• 
-1 . 5*max ( abs (x_hat_minus( 8, : ) ) ) 1. 5*max ( abs (x_hat_minus ( 8 , :)))]) 

orient tall 
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APPENDIX B: ERROR COVARIANCE MATRIX FOR SANS 
ASYNCHRONOUS KALMAN FILTER SIMULATION 




02D0 0200 0200 0200 0200 0200 0200 0200 



8x8 Matrix for Error Covariance P in SANS Kalman Filter Simulation 
Each box represents one element of the matrix vs time 
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